#pragma once
#include <highgui.h>
#include "stdlib.h"

using namespace std;

namespace featureTests
{

	void harrisTest(IplImage* source)
	{
		IplImage* gray = cvCreateImage(cvGetSize(source), 8,1);
		cvCvtColor( source, gray, CV_BGR2GRAY );

		IplImage* harris = cvCreateImage(cvGetSize(source), IPL_DEPTH_32F, 1);
		
		DWORD t1 = GetTickCount();
			cvCornerHarris(gray, harris, 3, 3, 0.004);
		printf("exctracting time: %i\r\n", GetTickCount() - t1);

		int count = 0;
		for (int y = 0; y < harris->height; y++)
            {
                for (int x = 0; x < harris->width; x++)
                {					
					float val = *((float*)(harris->imageData + harris->widthStep * y + sizeof(float) * x));
                    if (val > 0.001)
					{
						count++;
                        cvCircle(source, cvPoint(x, y), 2, CV_RGB(0,255,0));
					}
                }
            }
		printf("found %i features\r\n", count);
		cvReleaseImage(&gray);
		cvReleaseImage(&harris);
	}

	void goodFeaturesToTrackTest(IplImage* source)
	{
		IplImage* gray = cvCreateImage(cvGetSize(source), 8,1);
		cvCvtColor( source, gray, CV_BGR2GRAY );
		
		vector<cv::Point2f> points;
		points.reserve(200);
		DWORD t1 = GetTickCount();
		cv::goodFeaturesToTrack(gray, points, 100, 0.001, 5);
		printf("exctracting time: %i\r\n", GetTickCount() - t1);

		printf("found %i point good to track\r\n", points.size());

		for (int i = 0, max = points.size(); i < max; i++)
		{
			cv::Point2f* p = &(points[i]);
			cvCircle(source, cvPoint(p->x, p->y), 5, CV_RGB(0,255,0));
		}
		cvReleaseImage(&gray);
	}

	void test3(IplImage* source)
	{
		IplImage* gray = cvCreateImage(cvGetSize(source), 8,1);
		cvCvtColor( source, gray, CV_BGR2GRAY );
		
		CvSeq *objectKeypoints = 0, *objectDescriptors = 0;		
		CvMemStorage* storage = cvCreateMemStorage(0);

		CvSURFParams params = cvSURFParams(500, 1);
		cvExtractSURF( gray, 0, &objectKeypoints, &objectDescriptors, storage, params );			
		
		
		printf("Image Descriptors: %d\n", objectDescriptors->total);

		for(int i = 0; i < objectKeypoints->total; i++ )
		{
			CvSURFPoint* r = (CvSURFPoint*)cvGetSeqElem( objectKeypoints, i );
			CvPoint center;
			int radius;
			center.x = cvRound(r->pt.x);
			center.y = cvRound(r->pt.y);
			radius = cvRound(r->size*1.2/9.*2);
			cvCircle( source, center, radius, CV_RGB(0,255,0), 1, 8, 0 );
		}
	}

	void SurfAndGoodFeatureIntersecionTest(IplImage* source)
	{
		IplImage* gray = cvCreateImage(cvGetSize(source), 8,1);
		cvCvtColor( source, gray, CV_BGR2GRAY );
		
		IplImage* goodFeatireImage = cvCloneImage(source);
		IplImage* surfImage = cvCloneImage(source);
		IplImage* intersectionImage = cvCloneImage(source);
		
		// exctruct corers
		vector<cv::Point2f> points;
		cv::goodFeaturesToTrack(gray, points, 200, 0.001, 5);
		printf("found %i point good to track\r\n", points.size());

		for (int i = 0, max = points.size(); i < max; i++)
		{
			cv::Point2f* p = &(points[i]);
			cvCircle(goodFeatireImage, cvPoint(p->x, p->y), 5, CV_RGB(0,255,0));
		}

		cvShowImage("Good featire to track",goodFeatireImage);

		// now extract SURF
		CvSURFParams params = cvSURFParams(500, 1);
		CvSeq *objectKeypoints = 0, *objectDescriptors = 0;
	    CvMemStorage* storage = cvCreateMemStorage(0);

		cvExtractSURF( gray, 0, &objectKeypoints, &objectDescriptors, storage, params );
		printf("SURF Object Descriptors count: %d\n", objectDescriptors->total);

		int intersectionPointCount = 0;
		for(int i = 0; i < objectKeypoints->total; i++ )
		{
			CvSURFPoint* r = (CvSURFPoint*)cvGetSeqElem( objectKeypoints, i );
			CvPoint center;
			int radius;
			center.x = cvRound(r->pt.x);
			center.y = cvRound(r->pt.y);
			radius = cvRound(r->size*1.2/9.*2);
			cvCircle( surfImage, center, radius, CV_RGB(0,255,0));

			//and mark nearest result of two methods
			for (int i = 0, max = points.size(); i < max; i++)
			{
				cv::Point2f* p = &(points[i]);
				float d = sqrt(pow(p->x - r->pt.x, 2) + pow(p->y - r->pt.y, 2));
				if (d < 5)
				{
					// they are closely
					cvCircle( intersectionImage, center, radius, CV_RGB(0,255,0));
					cvCircle(intersectionImage, cvPoint(p->x, p->y),5,CV_RGB(255,255,255));
					intersectionPointCount++;
				}
			}
		}

		printf("Intersection set dimention: %i", intersectionPointCount);
		
		cvShowImage("SURF", surfImage);
		cvShowImage("Intersection", intersectionImage);

		cvReleaseImage(&goodFeatireImage);
		cvReleaseImage(&gray);
	}

	void test1()
	{
		cvNamedWindow("wnd");
		IplImage* img = cvLoadImage("images\\3_left.png");

		//http://opencv.willowgarage.com/documentation/cpp/feature_detection.html
		
		//SurfAndGoodFeatureIntersecionTest(img);
		//harrisTest(img);
		goodFeaturesToTrackTest(img);

		cvShowImage("wnd", img);
		cvWaitKey(0);
		
		cvReleaseImage(&img);
	}
}